# ffb_rc.py -主要实现巅峰极速dll加载、数据处理、数据发送

from ctypes import *
import os
import ctypes
import time
# from ffb_cal_0630 import ForceFeedbackAlgorithm
from ffb_cal_ori import ForceFeedbackAlgorithm
from loguru import logger
import math

import numpy as np
from numba import jit, njit, prange, float32, float64, int32
from retry_decorators import timing_analysis



# ============================
# 必要的结构体定义
# ============================

class FrameSuspensionData(Structure):
    _pack_ = 4
    _fields_ = [
        ("height", c_float),
        ("velocity", c_float),
    ]

class FrameTyreData(Structure):
    _pack_ = 4
    _fields_ = [
        ("slipRatio", c_float),
        ("slipAngle", c_float),
        ("load", c_float),
        ("rotationRate", c_float),
    ]

class FrameData(Structure):
    _pack_ = 4
    _fields_ = [
        ("clock", c_float),
        ("throttle", c_float),
        ("brake", c_float),
        ("gear", c_int),
        ("rpm", c_float),
        ("steering", c_float),
        ("speed", c_float),
        ("velocity", c_float * 3),
        ("acceleration", c_float * 3),
        ("yaw", c_float),
        ("pitch", c_float),
        ("roll", c_float),
        ("comy", c_float),
        ("tyreData", FrameTyreData * 4),
        ("suspensionData", FrameSuspensionData * 4),
        ("angular", c_float * 3),
        ("extraMessage", c_uint),
        ("running", c_uint),
        ("timeSinceLastCollision", c_float), # 自上次碰撞后的时间
    ]

class FrameInputData(Structure):
    _pack_ = 4
    _fields_ = [
        ("steer", c_float),
        ("throttle", c_float),
        ("brake", c_float),
        ("handbrake", c_float),
        ("clutch", c_float),
        ("gear", c_int),
    ]


# ============================
# 必要的函数指针定义
# ============================

RCInit = WINFUNCTYPE(None, c_bool)
RCReadFrameData = WINFUNCTYPE(None, POINTER(FrameData))
RCEnd = WINFUNCTYPE(None)
RCSetInputData = WINFUNCTYPE(None, POINTER(FrameInputData))


# ============================
# 必要的全局变量
# ============================

G_STEERING_WHEEL_ANGLE = 0.0
G_STEERING_RATE = 0.0
G_THROTTLE = 0.0
G_BRAKE = 0.0

#函数指针供外部访问
fRead = None
fEnd = None
fWriteInput= None
g112RC_api=None



def init_game_api():
    """
    使用 RacingRemoteController.dll 进行通信
    """
    global fRead, fEnd,fWriteInput,g112RC_api
    # dll_path = "RacingRemoteController(1).dll"
    try:
        # g112RC_api = WinDLL(dll_path)
        try:
            g112RC_api =windll.LoadLibrary(os.path.join(os.path.dirname(__file__), "RacingRemoteController(1).dll"))
        except Exception as e:
            #print(f"g112RC_api:{e}\n")
            logger.error(f"g112RC_api:{e}\n")

        fRead = RCReadFrameData(g112RC_api.RCReadFrameData)
        fWriteInput = RCSetInputData(g112RC_api.RCSetInputData)
        fEnd = RCEnd(g112RC_api.RCEnd)
        fInit = RCInit(g112RC_api.RCInit)
        fInit(False)
    except Exception as e:
        #print(f"Failed to load RC DLL: {e}")
        logger.error(f"Failed to load RC DLL: {e}")
        exit(0)


def get_fRead():
    """
    获取 fRead 函数引用。
    """
    if fRead is None:
        raise RuntimeError("fRead not initialized. Call init_game_api() first.")
    return fRead


def get_fEnd():
    """
    获取 fEnd 函数引用。
    """
    if fEnd is None:
        raise RuntimeError("fEnd not initialized. Call init_game_api() first.")
    return fEnd

def get_fWriteInput():
    """
    获取 fEnd 函数引用。
    """
    if fWriteInput is None:
        raise RuntimeError("fEnd not initialized. Call init_game_api() first.")
    return fWriteInput

@timing_analysis
def read_vehicle_status_from_rc():
    """
    使用 RacingRemoteController.dll 接口读取车辆状态数据
    """


    rc_date = FrameData()
    rc_date.roll = 0.0
    rc_date.pitch = 0.0
    # 使用 memset 清零数组部分（可选方法）
    rc_date.timeSinceLastCollision = -1.0  # 初始化为-1表示没有碰撞
    memset(rc_date.suspensionData, 0, sizeof(FrameSuspensionData) * 4)
    memset(rc_date.tyreData, 0, sizeof(FrameTyreData) * 4)
    memset(rc_date.acceleration, 0, sizeof(c_float) * 3)

    try:
        fRead(byref(rc_date))
    except Exception as e:
        #print(f"[ERROR] Failed to read frame data from RC: {e}")
        logger.error(f"[ERROR] Failed to read frame data from RC: {e}")
        return None, None, 0.0, None, None,None,None

    if rc_date.running == 0:
        #print("No data available from RC")
        logger.error("No data available from RC")
        return None, None, 0.0, None, None,None,None
    #print(f"============speed:{rc_date.speed}\n")
    logger.debug(f"============speed:{rc_date.speed}\n")
    # rc_date.speed=120

    return (
        rc_date.roll,
        rc_date.pitch,
        rc_date.speed,
        rc_date.suspensionData,
        rc_date.tyreData,
        rc_date.timeSinceLastCollision,
        rc_date.acceleration
    )


@timing_analysis
# @njit(parallel=False, fastmath=True)
def _calculate_suspension_force_moment_numba(suspension_data_array, wheel_base=2.5, track_width=1.5):
    """
    使用numba优化的悬架力矩计算
    """
    # 提取悬架高度数据
    left_front_height = suspension_data_array[0]
    right_front_height = suspension_data_array[1]
    left_rear_height = suspension_data_array[2]
    right_rear_height = suspension_data_array[3]
    
    # 计算前后悬架高度平均值
    front_height_avg = (left_front_height + right_front_height) / 2
    rear_height_avg = (left_rear_height + right_rear_height) / 2
    
    # 计算前后高度差（俯仰角度相关）
    pitch_diff = rear_height_avg - front_height_avg
    
    # 根据高度差计算力矩，映射到-1000至1000范围
    max_height_diff = 0.1  # 可根据需要调整
    force_moment = (pitch_diff / max_height_diff) * 1000
    
    # 限制输出范围在-1000至1000之间
    force_moment = max(-1000, min(1000, force_moment)) * 8
    
    return force_moment

@timing_analysis
def calculate_suspension_force_moment(suspension_data, wheel_base=2.5, track_width=1.5):
    """
    根据悬架数据计算悬架效应对方向盘的力矩
    
    参数:
    suspension_data: FrameSuspensionData * 4 数组，包含四个车轮的悬架数据
    wheel_base: 轴距(米)
    track_width: 轮距(米)
    
    返回:
    force_moment: 方向盘力矩 (N·m)，正值表示需要向右修正，负值表示需要向左修正
    """
    
    # 将悬架数据转换为numpy数组以供numba处理
    suspension_array = np.array([
        suspension_data[0].height,
        suspension_data[1].height,
        suspension_data[2].height,
        suspension_data[3].height
    ], dtype=np.float32)
    
    # 调用优化后的计算函数
    force_moment = _calculate_suspension_force_moment_numba(suspension_array, wheel_base, track_width)
    
    logger.info(f'force_moment: {force_moment}')
    
    return force_moment

@timing_analysis
# @njit(parallel=False, fastmath=True)
def _calculate_lateral_force_numba(slip_ratio, slip_angle, speed, g_effect_factor=1.5, tire_effect_factor=1.5):
    """
    使用numba优化的侧向力计算
    """
    # 速度因子，用于调节不同速度下的效果
    speed_factor = np.tanh(speed / 400)  # 调整分母以适应实际速度范围
    
    # G效应（侧向加速度效应）
    # 假设侧偏角代表侧向加速度影响，转换为弧度
    slip_angle_rad = np.radians(slip_angle)
    g_effect = slip_angle_rad * g_effect_factor
    
    # 轮胎效应（轮胎滑移效应）
    tire_effect = slip_ratio * tire_effect_factor
    
    # 计算总侧向力，映射到-1000到1000范围
    lateral_force_raw = (tire_effect + g_effect) * speed_factor * 1000
    
    # 限制输出范围在-1000至1000之间
    lateral_force = max(-1000, min(1000, lateral_force_raw)) * 0.8
    
    return lateral_force

@timing_analysis
def calculate_lateral_force(tyre_data, speed, acceleration, timeSinceLastCollision, friction_coefficient=1.0):
    """
    基于轮胎数据和车辆状态计算侧滑力（侧向力）
    
    参数:
    tyre_data: FrameTyreData 结构体实例
    speed: 车辆速度 (m/s)
    angular_vel_y: 车辆横摆角速度 (rad/s)
    friction_coefficient: 路面摩擦系数，默认为1.0
    
    返回:
    lateral_force: 侧向力 (N)，值在-1000至1000之间，绝对值小于300的置为0
    """
    
    # 调用优化后的计算函数
    lateral_force = _calculate_lateral_force_numba(
        tyre_data.slipRatio,
        tyre_data.slipAngle,
        speed,
        1.5,  # g_effect_factor
        1.5   # tire_effect_factor
    )
    
    logger.info(f'acceleration:{acceleration[0],acceleration[1],acceleration[2]}')
    logger.info(f" tire_effect: {tyre_data.slipRatio * 1.5}, g_effect: {np.radians(tyre_data.slipAngle) * 1.5}")
    logger.info(f" lateral_force: {lateral_force}")
    
    return lateral_force

@timing_analysis
# @njit(parallel=False, fastmath=True)
def _calculate_collision_force_numba(collision_speed, timeSinceLastCollision, previous_time, last_collision_force):
    """
    使用numba优化的碰撞力计算
    """
    # 只在碰撞后0.5秒内产生碰撞力
    if timeSinceLastCollision <= 0 or timeSinceLastCollision > 0.5:
        # 重置上次碰撞力和时间
        return 0.0, -1.0
    
    # 检查是否是新的碰撞（时间重置或首次调用）
    if timeSinceLastCollision < previous_time or previous_time < 0:
        # 新的碰撞发生，根据速度计算碰撞力的绝对值
        collision_speed_mph = 200  # 固定值，与原代码一致
        max_amplitude = min(0.2, (collision_speed_mph / 200.0) * 0.2)
        # 设置初始力为正值
        current_force = abs(max_amplitude)
    else:
        # 不是新碰撞，对上次的力取反
        current_force = -last_collision_force
    
    return current_force, timeSinceLastCollision

@timing_analysis
def calculate_collision_force(collision_speed, timeSinceLastCollision):
    """
    根据碰撞时间和碰撞发生时的速度计算碰撞力，只在碰撞后0.5秒内产生25Hz周期性力反馈
    碰撞力大小与碰撞发生时的速度线性相关
    
    参数:
    collision_speed: 碰撞发生时的速度 (km/h)
    timeSinceLastCollision: 距离上次碰撞的时间 (秒)
    
    返回:
    collision_force: 碰撞力 (N)，范围±0.05，在碰撞后0.5秒内有效
    """
    
    # 添加全局变量存储上次碰撞力值
    global last_collision_force, previous_time
    
    # 初始化全局变量
    if 'last_collision_force' not in globals():
        last_collision_force = 0.0
        previous_time = -1.0
    
    # 调用优化后的计算函数
    current_force, updated_time = _calculate_collision_force_numba(
        collision_speed,
        timeSinceLastCollision,
        previous_time,
        last_collision_force
    )
    
    # 如果返回了-1.0，表示需要重置
    if updated_time == -1.0:
        last_collision_force = 0.0
        previous_time = -1.0
        return 0.0
    
    # 更新全局变量
    last_collision_force = current_force
    previous_time = updated_time
    
    return current_force
def calculate_combined_tire_force(tyre_data, friction_coefficient=1.0):
    """
    综合考虑纵向力和侧向力的轮胎力计算
    
    参数:
    tyre_data: FrameTyreData 结构体实例
    friction_coefficient: 路面摩擦系数，默认为1.0
    
    返回:
    longitudinal_force: 纵向力 (N)
    lateral_force: 侧向力 (N)
    total_force: 合力 (N)
    """
    # 侧偏角转换为弧度
    slip_angle_rad = math.radians(tyre_data.slipAngle)
    
    # 简化的纵向力计算（基于滑移率）
    # 这里使用简化的魔术公式形式
    peak_slip_ratio = 0.1  # 峰值滑移率
    if tyre_data.slipRatio <= peak_slip_ratio:
        # 在峰值之前，纵向力与滑移率成正比
        longitudinal_force = tyre_data.load * friction_coefficient * (tyre_data.slipRatio / peak_slip_ratio)
    else:
        # 超过峰值后，纵向力逐渐减小
        longitudinal_force = tyre_data.load * friction_coefficient * max(0.2, 1.0 - (tyre_data.slipRatio - peak_slip_ratio) * 2)
    
    # 侧向力计算
    cornering_stiffness = 10000 * (tyre_data.load / 1000.0)
    lateral_force = cornering_stiffness * slip_angle_rad * friction_coefficient
    
    # 考虑滑移率对侧向力的影响
    if tyre_data.slipRatio > 0:
        slip_ratio_factor = max(0.1, 1.0 - tyre_data.slipRatio * 0.5)
        lateral_force *= slip_ratio_factor
    
    # 计算合力（纵向力和侧向力的矢量和）
    total_force = math.sqrt(longitudinal_force**2 + lateral_force**2)
    
    # 确保合力不超过最大摩擦力
    max_friction_force = tyre_data.load * friction_coefficient
    if total_force > max_friction_force:
        # 按比例缩放以符合摩擦力限制
        scale_factor = max_friction_force / total_force
        longitudinal_force *= scale_factor
        lateral_force *= scale_factor
        total_force = max_friction_force
    
    return longitudinal_force, lateral_force, total_force

